www.gusucode.com > 7-DOF Robotic Arm Model in MATLAB工具箱matlab源码 > 7-DOF Robotic Arm Model in MATLAB/threeD_error.m

     n=size(Time);

for i=1:n
    
    th1=PV_1(i);
    th2=PV_2(i);
    th3=PV_3(i);
    th4=PV_4(i);
    th5=PV_5(i);
    th6=PV_6(i);
    th7=PV_7(i);
    
    d1=0.36; d2=0.418; d3=0.4; d4=0.081; %arm architecture (m)

R1z=[cos(th1) -sin(th1) 0 0; sin(th1) cos(th1) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
T1z=[1 0 0 0;0 1 0 0;0 0 1 d1;0 0 0 1]; %Translation in z axis
R1x=[1 0 0 0;0 cos(-pi/2) -sin(-pi/2) 0;0 sin(-pi/2) cos(-pi/2) 0;0 0 0 1]; %Rotation matrix around x axis
A_0to1=R1z*T1z*R1x;
R2z=[cos(th2) -sin(th2) 0 0; sin(th2) cos(th2) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
R2x=[1 0 0 0;0 cos(pi/2) -sin(pi/2) 0;0 sin(pi/2) cos(pi/2) 0;0 0 0 1]; %Rotation matrix around x axis
A_1to2=R2z*R2x;
R3z=[cos(th3) -sin(th3) 0 0; sin(th3) cos(th3) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
T3z=[1 0 0 0;0 1 0 0;0 0 1 d2;0 0 0 1]; %Translation in z axis
R3x=[1 0 0 0;0 cos(pi/2) -sin(pi/2) 0;0 sin(pi/2) cos(pi/2) 0;0 0 0 1]; %Rotation matrix around x axis
A_2to3=R3z*T3z*R3x;
R4z=[cos(th4) -sin(th4) 0 0; sin(th4) cos(th4) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
R4x=[1 0 0 0;0 cos(-pi/2) -sin(-pi/2) 0;0 sin(-pi/2) cos(-pi/2) 0;0 0 0 1]; %Rotation matrix around x axis
A_3to4=R4z*R4x;
R5z=[cos(th5) -sin(th5) 0 0; sin(th5) cos(th5) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
T5z=[1 0 0 0;0 1 0 0;0 0 1 d3;0 0 0 1]; %Translation in z axis
R5x=[1 0 0 0;0 cos(-pi/2) -sin(-pi/2) 0;0 sin(-pi/2) cos(-pi/2) 0;0 0 0 1]; %Rotation matrix around x axis
A_4to5=R5z*T5z*R5x;
R6z=[cos(th6) -sin(th6) 0 0; sin(th6) cos(th6) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
R6x=[1 0 0 0;0 cos(pi/2) -sin(pi/2) 0;0 sin(pi/2) cos(pi/2) 0;0 0 0 1]; %Rotation matrix around x axis
A_5to6=R6z*R6x;
R7z=[cos(th7) -sin(th7) 0 0; sin(th7) cos(th7) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
T7z=[1 0 0 0;0 1 0 0;0 0 1 d4;0 0 0 1]; %Translation in z axis
A_6to7=R7z*T7z;

%A_0to7=A_0to1*A_1to2*A_2to3+A_3to4*A_4to5*A_5to6*A_6to7;

%%Dynamic model Kuka LWR robotic arm

A_0to2=A_0to1*A_1to2;
A_0to3=A_0to2*A_2to3;
A_0to4=A_0to3*A_3to4;
A_0to5=A_0to4*A_4to5;
A_0to6=A_0to5*A_5to6;
A_0to7=A_0to6*A_6to7;
    



Xval(i)=A_0to7(1,4);
Yval(i)=A_0to7(2,4);
Zval(i)=A_0to7(3,4);

end

for i=1:n
    
    th1=SP_1(i);
    th2=SP_2(i);
    th3=SP_3(i);
    th4=SP_4(i);
    th5=SP_5(i);
    th6=SP_6(i);
    th7=SP_7(i);
    
R1z=[cos(th1) -sin(th1) 0 0; sin(th1) cos(th1) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
T1z=[1 0 0 0;0 1 0 0;0 0 1 d1;0 0 0 1]; %Translation in z axis
R1x=[1 0 0 0;0 cos(-pi/2) -sin(-pi/2) 0;0 sin(-pi/2) cos(-pi/2) 0;0 0 0 1]; %Rotation matrix around x axis
A_0to1=R1z*T1z*R1x;
R2z=[cos(th2) -sin(th2) 0 0; sin(th2) cos(th2) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
R2x=[1 0 0 0;0 cos(pi/2) -sin(pi/2) 0;0 sin(pi/2) cos(pi/2) 0;0 0 0 1]; %Rotation matrix around x axis
A_1to2=R2z*R2x;
R3z=[cos(th3) -sin(th3) 0 0; sin(th3) cos(th3) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
T3z=[1 0 0 0;0 1 0 0;0 0 1 d2;0 0 0 1]; %Translation in z axis
R3x=[1 0 0 0;0 cos(pi/2) -sin(pi/2) 0;0 sin(pi/2) cos(pi/2) 0;0 0 0 1]; %Rotation matrix around x axis
A_2to3=R3z*T3z*R3x;
R4z=[cos(th4) -sin(th4) 0 0; sin(th4) cos(th4) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
R4x=[1 0 0 0;0 cos(-pi/2) -sin(-pi/2) 0;0 sin(-pi/2) cos(-pi/2) 0;0 0 0 1]; %Rotation matrix around x axis
A_3to4=R4z*R4x;
R5z=[cos(th5) -sin(th5) 0 0; sin(th5) cos(th5) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
T5z=[1 0 0 0;0 1 0 0;0 0 1 d3;0 0 0 1]; %Translation in z axis
R5x=[1 0 0 0;0 cos(-pi/2) -sin(-pi/2) 0;0 sin(-pi/2) cos(-pi/2) 0;0 0 0 1]; %Rotation matrix around x axis
A_4to5=R5z*T5z*R5x;
R6z=[cos(th6) -sin(th6) 0 0; sin(th6) cos(th6) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
R6x=[1 0 0 0;0 cos(pi/2) -sin(pi/2) 0;0 sin(pi/2) cos(pi/2) 0;0 0 0 1]; %Rotation matrix around x axis
A_5to6=R6z*R6x;
R7z=[cos(th7) -sin(th7) 0 0; sin(th7) cos(th7) 0 0;0 0 1 0;0 0 0 1]; %Rotation matrix around z axis
T7z=[1 0 0 0;0 1 0 0;0 0 1 d4;0 0 0 1]; %Translation in z axis
A_6to7=R7z*T7z;

%A_0to7=A_0to1*A_1to2*A_2to3+A_3to4*A_4to5*A_5to6*A_6to7;

%%Dynamic model Kuka LWR robotic arm

A_0to2=A_0to1*A_1to2;
A_0to3=A_0to2*A_2to3;
A_0to4=A_0to3*A_3to4;
A_0to5=A_0to4*A_4to5;
A_0to6=A_0to5*A_5to6;
A_0to7=A_0to6*A_6to7;
   

X(i)=A_0to7(1,4);
Y(i)=A_0to7(2,4);
Z(i)=A_0to7(3,4);

end

figure(15)
plot(Time,X,Time,Xval);
title('Position X axis SP-PV');
xlabel('Time (s)');
ylabel('Position (m)');
legend('SP','PV','Location','southeast');
figure(16)
plot(Time,Y,Time,Yval);
title('Position Y axis SP-PV');
xlabel('Time (s)');
ylabel('Position (m)');
legend('SP','PV','Location','southeast');
figure(17)
plot(Time,Z,Time,Zval);
title('Position Z axis SP-PV');
xlabel('Time (s)');
ylabel('Position (m)');
legend('SP','PV','Location','southeast');
figure(18);
plot3(X,Y,Z);
title('End-effector trajectory'); title('End-effector trajectory');
xlabel('Position (m)');
ylabel('Position (m)');
zlabel('Position (m)');

Xerr=X-Xval;
Yerr=Y-Yval;
Zerr=Z-Zval;

figure(19)
plot(Time,Xerr);
title('Error X axis');
xlabel('Time (s)');
ylabel('Error (m)');
legend('Error','Location','southeast');
figure(20)
plot(Time,Yerr);
title('Error Y axis');
xlabel('Time (s)');
ylabel('Error (m)');
legend('Error','Location','southeast');
figure(21)
plot(Time,Zerr);
title('Error Z axis');
xlabel('Time (s)');
ylabel('Error (m)');
legend('Error','Location','southeast');

for i=1:n
Q=Xerr(i)^2+Yerr(i)^2+Zerr(i)^2;    
E(i)=nthroot(Q,3); 
end

figure(22)
plot(Time,E);
title('Absolute error');
xlabel('Time (s)');
ylabel('Error (m)');
legend('Error','Location','southeast')